
#include "registration.h"
#include <omp.h>





void
initialRegisterClustersTrySACAll1 (
        PointCloud<PointXYZ>::Ptr object,
        PointCloud<PointXYZ>::Ptr world,
        PointCloud<PointXYZI>::Ptr object_keypoints,
        vector<Alignment*> &alignmentAttempts,
        regConfig config)
{
    int millistart;
    int span;
    millistart= getMilliCount();
    //COMPUTAZIONE ELEMENTI OBJECT
    PointCloud<PointXYZI>::Ptr totalWorld_keypoints(new PointCloud<PointXYZI>);
    uniformKeypoints(world,totalWorld_keypoints,config.initialResolution);
    uniformKeypoints(object,object_keypoints,config.initialResolution);
    span =getMilliSpan(millistart);
    cout<<"Keypoints milliseconds: *******************************************************************************"<<span<<endl;
        millistart= getMilliCount();
    vector<PointIndices> worldClusters = findClusters(*world,config.clusterThreshold,object->size()*config.minClusterSize,object->size()*config.maxClusterSize);//un quarto o meta' dell'oggetto come minima dimensione
    span =getMilliSpan(millistart);
    cout<<"Cluster computation milliseconds: *******************************************************************************"<<span<<endl;
    vector<bool> hasSkippedCluster;
    for (int i=0;i<worldClusters.size();i++)//un alignment attempt per ogni cluster
    {
        alignmentAttempts.push_back(new Alignment());
        hasSkippedCluster.push_back(false);
    }
#pragma omp parallel for if(config.parallel) schedule(dynamic)
    for (int i=0;i<alignmentAttempts.size();i++)//un alignment attempt per ogni cluster
    {
        PointCloud<PointXYZ>::Ptr currentWorldCluster(new PointCloud<PointXYZ>);
        copyPointCloud(*world,worldClusters.at(i).indices,*currentWorldCluster);//currentWorldCluster contiene il cluster i-esimo
        uniformKeypoints(currentWorldCluster,alignmentAttempts.at(i)->getWorld_keypoints(),config.initialResolution);
    }

        span=0;
        int millistart2=0;
        int span2=0;
        int millistart3=0;
        int span3=0;
        int millistart4=0;
        int span4=0;
        int millistart5=0;
        int span5=0;
    for (double radiusFPFH=config.minRadiusFPFH;radiusFPFH<config.maxRadiusFPFH;radiusFPFH+=config.stepRadiusFPFH)//per ogni raggio FPFH
    {
        cout <<"FPFH RADIUS "<<radiusFPFH<<endl;
        PointCloud<FPFHSignature33>::Ptr object_features (new PointCloud<FPFHSignature33>);
        millistart= getMilliCount();
        fpfhWithUniformKeypoints (object_keypoints, object_features,radiusFPFH);
        span +=getMilliSpan(millistart);
#pragma omp parallel for if(config.parallel) schedule(dynamic)
        for (int i=0;i<worldClusters.size();i++)//per ogni cluster
        {
            if(hasSkippedCluster.at(i))continue;//SE NON TROVA CORRISPONDENZE CON RAGGIO X ALLORA NON NE TROVA NEANCHE CON Y>X
            Eigen::Matrix4f currentInitial_transformation_matrix = Eigen::Matrix4f::Identity();
            float currentScore=FLT_MAX;
            //COMPUTAZIONE ELEMENTI WORLD
            PointCloud<FPFHSignature33>::Ptr world_features (new PointCloud<FPFHSignature33>);
             millistart= getMilliCount();
            fpfhWithUniformKeypoints (alignmentAttempts.at(i)->getWorld_keypoints(), world_features,radiusFPFH);
            span +=getMilliSpan(millistart);
            //FIND CORRESPONDENCES
            CorrespondencesPtr totalClusterCorrespond(new Correspondences);
            millistart3= getMilliCount();
            findCorrespondencesFPFH(object_features,world_features,totalClusterCorrespond);
            span3 +=getMilliSpan(millistart3);
            cout<<"Cluster: "<<i<<" - Total: correspondences: "<<totalClusterCorrespond->size()<<endl;
            if(totalClusterCorrespond->size()<=3)
            {//salta il giro se non ci sono almeno 3 corrispond (trasformazione rigida)
                cout<<"Cluster: "<<i<<" - Not enough correspondences found. Skipping cluster"<<endl;
                hasSkippedCluster.at(i)=true;
                continue;
            }
            //ORA DEVO PROVARE DIVERSE SOGLIE DI FILTRO SAC SU QUESTE CORRISPONDENZE TROVATE
            //TRANSFORM ESTIMATION
            bool hasChanged=false;
            registration::TransformationEstimationSVD<PointXYZI, PointXYZI>::Ptr transformation_estimation (new registration::TransformationEstimationSVD<PointXYZI, PointXYZI>);

            for(double thresh=config.maxSACThresh;thresh>=config.minSACThresh;thresh-=config.stepSACThresh)//per ogni threshold
            {
                CorrespondencesPtr currentCorrespond(new Correspondences);
                *currentCorrespond=*totalClusterCorrespond;
                //FILTER CORRESPONDENCES
                millistart2= getMilliCount();
                filterCorrespondencesSAC(object_keypoints,alignmentAttempts.at(i)->getWorld_keypoints(),currentCorrespond,thresh);
                span2 +=getMilliSpan(millistart2);
                if(currentCorrespond->size()<=3)//salta il giro se non ci sono almeno 3 corrispond (trasformazione rigida)
                {
                    break;//cout<<"Not enough correspondences filtered. Skipping threshold "<< thresh <<endl;
                }
                millistart4= getMilliCount();
                transformation_estimation->estimateRigidTransformation (*object_keypoints, * (alignmentAttempts.at(i)->getWorld_keypoints()), *currentCorrespond, currentInitial_transformation_matrix);
                span4 +=getMilliSpan(millistart4);
                millistart5= getMilliCount();
                //USO TUTTA LA WORLD CLOUD
                currentScore=validationScoreProximityTargetApplyTransformation(*totalWorld_keypoints,*object_keypoints,currentInitial_transformation_matrix);
                //USO SOLO IL CLUSTER (SI GUADAGNA UN SECONDO!) currentScore=validationScoreProximityTargetApplyTransformation(*(alignmentAttempts.at(i)->getWorld_keypoints()),*object_keypoints,currentInitial_transformation_matrix);
                span5 +=getMilliSpan(millistart5);
                //cout << "Thresh: "<< thresh <<" - Correspond filtered: "<< currentCorrespond->size() <<" - Score: "<< currentScore <<endl;
                if (currentScore<alignmentAttempts.at(i)->getInitialScore())
                {
                    alignmentAttempts.at(i)->setInitialScore(currentScore);
                    alignmentAttempts.at(i)->setInitial_transformation_matrix(currentInitial_transformation_matrix);
                    alignmentAttempts.at(i)->setCorrespond(currentCorrespond);//POI VIENE MODIFICATO CURRENT CORRESPOND, CREARE COPIA...
                    hasChanged=true;
                }
            }//END FOR EACH THRESHOLD

            if(hasChanged)
                cout <<"Cluster: "<<i<<" - Correspondences used: " << alignmentAttempts.at(i)->getCorrespond()->size()<< " - Better score: "<<alignmentAttempts.at(i)->getInitialScore() <<endl;
            else
                cout <<"Cluster: "<<i<<" - Not changed" <<endl;

            //DEBUG cout<<correspond.at(i)->at(0).index_match<<" / "<<correspond.at(i)->at(0).index_query<<flush;
        }//END FOR EACH CLUSTER
    }//END FOR EACH RADIUS FPFH
    cout<<"FPFH computation milliseconds: *******************************************************************************"<<span<<endl;
    cout<<"correspond on FPFH milliseconds *******************************************************************************"<<span3<<endl;
    cout<<"SAC filtering milliseconds *******************************************************************************"<<span2<<endl;
    cout<<"Transform estimation milliseconds"<<span4<<endl;
    cout<<"Euclidean Validation function milliseconds *******************************************************************************"<<span5<<endl;
}

void
initialRegisterClustersTrySACAll2 (
        PointCloud<PointXYZ>::Ptr object,
        PointCloud<PointXYZ>::Ptr world,
        PointCloud<PointXYZI>::Ptr object_keypoints,
        vector<Alignment*> &alignmentAttempts,
        regConfig config)
{
    //COMPUTAZIONE ELEMENTI OBJECT
    PointCloud<PointXYZI>::Ptr totalWorld_keypoints(new PointCloud<PointXYZI>);
    uniformKeypoints(world,totalWorld_keypoints,config.initialResolution);
    uniformKeypoints(object,object_keypoints,config.initialResolution);
    vector<PointIndices> worldClusters = findClusters(*world,config.clusterThreshold,object->size()*config.minClusterSize,object->size()*config.maxClusterSize);//un quarto o meta' dell'oggetto come minima dimensione
    for (int i=0;i<worldClusters.size();i++)//un alignment attempt per ogni cluster
    {
        alignmentAttempts.push_back(new Alignment());
    }
#pragma omp parallel for if(config.parallel) schedule(dynamic)
    for (int i=0;i<alignmentAttempts.size();i++)//un alignment attempt per ogni cluster
    {
        PointCloud<PointXYZ>::Ptr currentWorldCluster(new PointCloud<PointXYZ>);
        copyPointCloud(*world,worldClusters.at(i).indices,*currentWorldCluster);//currentWorldCluster contiene il cluster i-esimo
        uniformKeypoints(currentWorldCluster,alignmentAttempts.at(i)->getWorld_keypoints(),config.initialResolution);
    }

    vector<PointCloud<FPFHSignature33>::Ptr > objectFeatures;
    vector<double> radiiFPFH;
    for (double radiusFPFH=config.minRadiusFPFH;radiusFPFH<config.maxRadiusFPFH;radiusFPFH+=config.stepRadiusFPFH)//per ogni raggio FPFH
    {
        radiiFPFH.push_back(radiusFPFH);
        objectFeatures.push_back(boost::shared_ptr<PointCloud<FPFHSignature33> > (new PointCloud<FPFHSignature33> ));
    }
    #pragma omp parallel for if(config.parallel) schedule(dynamic)
    for (int i=0;i<radiiFPFH.size();i++)
        fpfhWithUniformKeypoints (object_keypoints, objectFeatures.at(i),radiiFPFH.at(i));
    cout << "FPFH Object OK!" <<endl;

#pragma omp parallel for if(config.parallel) schedule(dynamic)
    for (int i=0;i<worldClusters.size();i++)//per ogni cluster
    {
        int h=0;
        //FIND CORRESPONDENCES
        CorrespondencesPtr totalClusterCorrespond(new Correspondences);
        for (double radiusFPFH=config.minRadiusFPFH;radiusFPFH<config.maxRadiusFPFH;radiusFPFH+=config.stepRadiusFPFH)//per ogni raggio FPFH
        {
            PointCloud<FPFHSignature33>::Ptr object_features = objectFeatures.at(h++);
            PointCloud<FPFHSignature33>::Ptr world_features (new PointCloud<FPFHSignature33>);
            fpfhWithUniformKeypoints (alignmentAttempts.at(i)->getWorld_keypoints(), world_features,radiusFPFH);
            CorrespondencesPtr partialClusterCorrespond(new Correspondences);
            findCorrespondencesFPFH(object_features,world_features,partialClusterCorrespond);
            addCorrespondencesToResult(partialClusterCorrespond,totalClusterCorrespond);
        }//END FOR EACH RADIUS FPFH
        removeClones(totalClusterCorrespond);
        if(totalClusterCorrespond->size()<=3)
        {//salta il giro se non ci sono almeno 3 corrispond (trasformazione rigida)
            cout<<"Cluster: "<<i<<" - Not enough correspondences found. Skipping cluster"<<endl;
            continue;
        }
        //ORA DEVO PROVARE DIVERSE SOGLIE DI FILTRO SAC SU QUESTE CORRISPONDENZE TROVATE
        Eigen::Matrix4f currentInitial_transformation_matrix = Eigen::Matrix4f::Identity();
        float currentScore=FLT_MAX;
        registration::TransformationEstimationSVD<PointXYZI, PointXYZI>::Ptr transformation_estimation (new registration::TransformationEstimationSVD<PointXYZI, PointXYZI>);
        for(double thresh=config.maxSACThresh;thresh>=config.minSACThresh;thresh-=config.stepSACThresh)//per ogni threshold
        {
            CorrespondencesPtr currentCorrespond(new Correspondences);
            *currentCorrespond=*totalClusterCorrespond; //CREATA COPIA PERCHE' VEDI SOTTO!
            //FILTER CORRESPONDENCES
            filterCorrespondencesSAC(object_keypoints,alignmentAttempts.at(i)->getWorld_keypoints(),currentCorrespond,thresh);

            if(currentCorrespond->size()<=3)//salta il giro se non ci sono almeno 3 corrispond (trasformazione rigida)
            {
                break;//cout<<"Not enough correspondences filtered. Skipping threshold "<< thresh <<endl;
            }
            transformation_estimation->estimateRigidTransformation (*object_keypoints, * (alignmentAttempts.at(i)->getWorld_keypoints()), *currentCorrespond, currentInitial_transformation_matrix);
            //currentScore=validationScoreProximityTargetApplyTransformation(*totalWorld_keypoints,*object_keypoints,currentInitial_transformation_matrix);
            //OLD ONE
            currentScore=validationScoreProximityTargetApplyTransformation(*(alignmentAttempts.at(i)->getWorld_keypoints()),*object_keypoints,currentInitial_transformation_matrix);
            if (currentScore<alignmentAttempts.at(i)->getInitialScore())
            {
                alignmentAttempts.at(i)->setInitialScore(currentScore);
                alignmentAttempts.at(i)->setInitial_transformation_matrix(currentInitial_transformation_matrix);
                alignmentAttempts.at(i)->setCorrespond(currentCorrespond);//POI VIENE MODIFICATO CURRENT CORRESPOND, CREARE COPIA...
            }
        }//END FOR EACH THRESHOLD
        cout <<"Cluster: "<<i<<" - FPFH OK! Correspondences found: "<<totalClusterCorrespond->size()<<" used: " << alignmentAttempts.at(i)->getCorrespond()->size()<< " - Initial score: "<<alignmentAttempts.at(i)->getInitialScore() <<endl;


        //DEBUG cout<<correspond.at(i)->at(0).index_match<<" / "<<correspond.at(i)->at(0).index_query<<flush;
    }//END FOR EACH CLUSTER



}



//non modifica le cloud
vector<int> finalRegister(
        PointCloud<PointXYZ>::Ptr world,
        Alignment *alignmentAttempt,
        regConfig config)
{
    PointCloud<PointXYZI>::Ptr object_keypoints (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr object_keypoints_registered (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr world_keypoints (new PointCloud<PointXYZI>);
    uniformKeypoints(alignmentAttempt->getInitialObjectRegistered(),object_keypoints,config.finalResolution);
    uniformKeypoints(world,world_keypoints,config.finalResolution);
    float initialFitnessScore = validationScoreProximityTargetPercent(*world_keypoints,*object_keypoints);
    cutShapeAroundTarget(*object_keypoints,*world_keypoints);
    IterativeClosestPoint<PointXYZI, PointXYZI>::Ptr registration (new IterativeClosestPoint<PointXYZI, PointXYZI>);
    registration->setInputCloud(object_keypoints);
    registration->setInputTarget (world_keypoints);
    registration->setRANSACIterations(100);//default 1000
    registration->setMaximumIterations (100);     //2 TermCriterion  Number of iterations has reached the maximum
    registration->setTransformationEpsilon (1e-7);//3 TermCriterion  The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than parameter
    float finalFitnessScore = FLT_MAX;
    Eigen::Matrix4f final_transformation_matrix = Eigen::Matrix4f::Identity();
    vector<int> spans;
    int a=0;
    int b=0;
    spans.push_back(a);    spans.push_back(b);
    int millistart=0;
    int millistart2=0;
    for(double thresh=5;thresh>2;thresh-=5)//per ogni threshold
    {
        object_keypoints_registered->clear();
        registration->setRANSACOutlierRejectionThreshold (thresh);//0.05 default  Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
        millistart= getMilliCount();
        registration->align(*object_keypoints_registered);//applica la trasformazione
        spans.at(0) += getMilliSpan(millistart);
        millistart2= getMilliCount();
        float currentScore = validationScoreProximityTargetPercent(*world_keypoints,*object_keypoints_registered);
        spans.at(1) += getMilliSpan(millistart2);
        /*
        float score;
        int start;
        int milliSecondsElapsed;
                //EUCLIDEAN
                start = getMilliCount();
                score = validationScoreProximityTargetEuclidean(*world_keypoints,*object_keypoints_registered);
                milliSecondsElapsed = getMilliSpan(start);
                cout <<"---- validationScoreProximityTargetEuclidean = "<<score<<" Time = "<<milliSecondsElapsed<<endl;
                //ANGLES
                start = getMilliCount();
                score = validationScoreProximityTargetAngles(*world_keypoints,*object_keypoints_registered);
                milliSecondsElapsed = getMilliSpan(start);
                cout <<"---- validationScoreProximityTargetAngles = "<<score<<" Time = "<<milliSecondsElapsed<<endl;
                //PERCENT
                start = getMilliCount();
                score = validationScoreProximityTargetPercent(*world_keypoints,*object_keypoints_registered);
                milliSecondsElapsed = getMilliSpan(start);
                cout <<"---- validationScoreProximityTargetPercent = "<<score<<" Time = "<<milliSecondsElapsed<<endl;*/


        if (currentScore<finalFitnessScore)
        {
            finalFitnessScore = currentScore;
            final_transformation_matrix = registration->getFinalTransformation();
        }
    }
    if(initialFitnessScore>finalFitnessScore)//controllo se ICP ha migliorato o peggiorato il risultato
    {
        transformPointCloud(
                    * (alignmentAttempt->getInitialObjectRegistered()),
                    * (alignmentAttempt->getObjectRegistered()),
                    alignmentAttempt->getFinal_transformation_matrix());
        alignmentAttempt->setFinal_transformation_matrix( final_transformation_matrix);
        alignmentAttempt->setFinalScore(finalFitnessScore);
    }
    else
    {
        alignmentAttempt->setObjectRegistered( alignmentAttempt->getInitialObjectRegistered() );
        alignmentAttempt->setFinalScore(initialFitnessScore);
    }
    cout << "OK! Final Score: " << alignmentAttempt->getFinalScore()<<endl;
    return spans;
}



void
registerAllWithAlignment(
        PointCloud<PointXYZ>::Ptr object ,
        PointCloud<PointXYZ>::Ptr world,
        vector<Alignment*> &alignmentAttempts,
        PointCloud<PointXYZI>::Ptr object_keypoints,
        regConfig config)
{
    time_t start,end;
    time (&start);
      int millistart1=0;
    int millistart2=0;
    int spanA=0;
    int spanB=0;
    vector<Alignment*> initialAlignmentAttempts;
    vector<Alignment*> finalAlignmentAttempts;
    //-----------------------INITIAL PROCEDURE
    millistart1= getMilliCount();
    cout << "INITIAL TRANSFORMATION -------------------------------- Initial Leaf Size: " << config.initialResolution <<endl;

    if (config.initialAlgorithm==1)
        initialRegisterClustersTrySACAll1(
                    object,
                    world,
                    object_keypoints,
                    initialAlignmentAttempts,
                    config);
    else if (config.initialAlgorithm==2)
        initialRegisterClustersTrySACAll2(
                    object,
                    world,
                    object_keypoints,
                    initialAlignmentAttempts,
                    config);
    else
    {
        cout <<"Wrong initial Algorithm selected. Choose 1 or 2"<<endl; exit(1);
    }
    spanA =getMilliSpan(millistart1);
    cout<<"Initial milliseconds------------------------> "<<spanA<<endl;
    cout << "FILTERING BEST INITIAL ALIGNMENTS -------------------------------- Initial Score Threshold: " << config.initialScoreThreshold <<endl;
    for(int i=0;i<initialAlignmentAttempts.size();i++)//cernita degli allineamenti iniziali migliori
    {
        if(initialAlignmentAttempts.at(i)->getInitialScore()<config.initialScoreThreshold) //salta giro se score iniziale troppo alto
        {
            cout <<"Cluster: "<<i<< " - OK.        Initial score: "<<initialAlignmentAttempts.at(i)->getInitialScore()<<endl;
            finalAlignmentAttempts.push_back(initialAlignmentAttempts.at(i));
        }
        else
            cout <<"Cluster: "<<i<< " - Rejected . Initial score: "<<initialAlignmentAttempts.at(i)->getInitialScore()<<endl;
    }
    cout << "FINAL TRANSFORMATIONS ------------------------------- Final Leaf Size: " << config.finalResolution <<endl;

int span=0;
int span2=0;
    millistart2= getMilliCount();
#pragma omp parallel for if(config.parallel) schedule(dynamic)
    for(int i=0;i<finalAlignmentAttempts.size();i++)
    {
        transformPointCloud(
                    *object,
                    *( finalAlignmentAttempts.at(i)->getInitialObjectRegistered() ),
                    finalAlignmentAttempts.at(i)->getInitial_transformation_matrix());
        //-----------------------FINAL PROCEDURE
        vector<int> spans = finalRegister(
                    world,
                    finalAlignmentAttempts.at(i),
                    config);
        transformPointCloud(
                    *( finalAlignmentAttempts.at(i)->getInitialObjectRegistered()),
                    *( finalAlignmentAttempts.at(i)->getObjectRegistered()),
                    finalAlignmentAttempts.at(i)->getFinal_transformation_matrix());
        //------------------------SET MATRIX
        finalAlignmentAttempts.at(i)->setTransformation_matrix(
                    finalAlignmentAttempts.at(i)->getInitial_transformation_matrix()*
                    finalAlignmentAttempts.at(i)->getFinal_transformation_matrix());
        span+=spans.at(0);
        span2+=spans.at(1);
    }

    cout<<"ICP milliseconds: *******************************************************************************"<<span<<endl;
    cout<<"Percent Validation function milliseconds: *******************************************************************************"<<span2<<endl;
    spanB =getMilliSpan(millistart2);
    cout<<"Final milliseconds------------------------> "<<spanB<<endl;
    cout << "FILTERING BEST FINAL ALIGNMENTS -------------------------------- Final Score Threshold: " << config.finalScoreThreshold <<endl;
    for(int i=0;i<finalAlignmentAttempts.size();i++)//cernita degli allineamenti iniziali migliori
    {
        if(finalAlignmentAttempts.at(i)->getFinalScore()<config.finalScoreThreshold) //salta giro se score iniziale troppo alto
        {
            cout <<"Object OK.       Final score: "<<finalAlignmentAttempts.at(i)->getFinalScore()<<endl;
            alignmentAttempts.push_back(finalAlignmentAttempts.at(i));
        }
        else
            cout <<"Object rejected. Final score: "<<finalAlignmentAttempts.at(i)->getFinalScore()<<endl;

        /*
float score;
int start;
int milliSecondsElapsed;
        //EUCLIDEAN
        start = getMilliCount();
        score = validationScoreProximityTargetEuclidean(*world,*(finalAlignmentAttempts.at(i)->getObjectRegistered()));
        milliSecondsElapsed = getMilliSpan(start);
        cout <<"#### validationScoreProximityTargetEuclidean = "<<score<<" Time = "<<milliSecondsElapsed<<endl;
        //ANGLES
        start = getMilliCount();
        score = validationScoreProximityTargetAngles(*world,*(finalAlignmentAttempts.at(i)->getObjectRegistered()));
        milliSecondsElapsed = getMilliSpan(start);
        cout <<"#### validationScoreProximityTargetAngles = "<<score<<" Time = "<<milliSecondsElapsed<<endl;
        //PERCENT
        start = getMilliCount();
        score = validationScoreProximityTargetPercent(*world,*(finalAlignmentAttempts.at(i)->getObjectRegistered()));
        milliSecondsElapsed = getMilliSpan(start);
        cout <<"#### validationScoreProximityTargetPercent = "<<score<<" Time = "<<milliSecondsElapsed<<endl;
*/
    }
    time (&end);
    double dif = difftime (end,start);
    cout << "------------------------------------->  Total registration in " << dif << " seconds"<< endl;

}





